#include "SensorSuite.h"

void SensorSuite::begin(StorageManager *storage) {
  _storage = storage;
  _dht.begin();
  pinMode(Pins::ULTRA_TRIG, OUTPUT);
  pinMode(Pins::ULTRA_ECHO, INPUT);
}

float SensorSuite::readRawDistanceCm() {
  digitalWrite(Pins::ULTRA_TRIG, LOW); delayMicroseconds(2);
  digitalWrite(Pins::ULTRA_TRIG, HIGH); delayMicroseconds(10);
  digitalWrite(Pins::ULTRA_TRIG, LOW);
  long duration = pulseIn(Pins::ULTRA_ECHO, HIGH, 25000UL); // 25ms timeout (~4m range)
  if (duration == 0) return 400.0f; // no echo 
  return duration * 0.0343f / 2.0f;
}

float SensorSuite::soilRawToPct(int raw) const {
  int dryRaw = _storage ? _storage->soilDryRaw() : 900;
  int wetRaw = _storage ? _storage->soilWetRaw() : 300;
  if (dryRaw == wetRaw) return 50.0f; // guard against bad calibration (div by zero)
  float pct = 100.0f * (float)(dryRaw - raw) / (float)(dryRaw - wetRaw);
  return constrain(pct, 0.0f, 100.0f);
}

float SensorSuite::batteryPct() const {
  float v = _batteryFilter.get();
  float pct = 100.0f * (v - Power::BATTERY_EMPTY_V) / (Power::BATTERY_FULL_V - Power::BATTERY_EMPTY_V);
  return constrain(pct, 0.0f, 100.0f);
}

void SensorSuite::poll(unsigned long nowMs) {
  (void)nowMs;

  float h = _dht.readHumidity();
  if (!isnan(h)) _humidity = h; // on a failed DHT read, keep the last good value

  float lightRaw = (analogRead(Pins::LIGHT) / 1023.0f) * 100.0f;
  _lightFilter.update(lightRaw);

  int soilRawAdc = analogRead(Pins::SOIL);
  _soilFilter.update(soilRawToPct(soilRawAdc));

  float battRaw = (analogRead(Pins::BATTERY) / 1023.0f) * 5.0f * (Power::BATTERY_FULL_V / 5.0f);
  _batteryFilter.update(battRaw);

  float distRaw = readRawDistanceCm();
  _distanceFiltered = _distanceMedian.update(distRaw);

  bool soilOK  = soilPct() >= Thresholds::SOIL_MIN_PCT && soilPct() <= Thresholds::SOIL_MAX_PCT;
  bool humOK   = _humidity >= Thresholds::HUMIDITY_MIN_PCT && _humidity <= Thresholds::HUMIDITY_MAX_PCT;
  bool lightOK = lightPct() >= Thresholds::LIGHT_MIN_PCT;
  bool instSuitable = soilOK && humOK && lightOK;

  if (instSuitable) {
    if (_suitableStreak < 255) _suitableStreak++;
    _unsuitableStreak = 0;
    if (_suitableStreak >= Thresholds::SUITABILITY_DEBOUNCE_READS) _suitableLatched = true;
  } else {
    if (_unsuitableStreak < 255) _unsuitableStreak++;
    _suitableStreak = 0;
    if (_unsuitableStreak >= Thresholds::SUITABILITY_DEBOUNCE_READS) _suitableLatched = false;
  }
}

void SensorSuite::calibrateSoilDry() {
  long sum = 0;
  for (uint8_t i = 0; i < 8; i++) { sum += analogRead(Pins::SOIL); delay(20); }
  int raw = sum / 8;
  if (_storage) _storage->setSoilCalibration(raw, _storage->soilWetRaw());
}

void SensorSuite::calibrateSoilWet() {
  long sum = 0;
  for (uint8_t i = 0; i < 8; i++) { sum += analogRead(Pins::SOIL); delay(20); }
  int raw = sum / 8;
  if (_storage) _storage->setSoilCalibration(_storage->soilDryRaw(), raw);
}
